#include <webots/Robot.hpp>
#include <webots/Supervisor.hpp>
#include <webots/Field.hpp>
#include <webots/Node.hpp>


#include <ros/ros.h>
#include <math.h>
//#include <string.hpp>
#include <stdio.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Image.h>

#include "WDynObjMarker.hpp"

#include "CSUP/DynamicObjectMsg.h"
#include "CSUP/DetectedObjectsMsg.h"

// %EndTag(MSG_HEADER)%

#include <sstream>


#include <iostream>
#include <string>
#include <cstring>

using namespace std;
using namespace webots;


class RWSDynObjs
{
public:
  RWSDynObjs();
  void init(webots::Supervisor*, ros::NodeHandle* ,string);
  void DynObjList_CallBack(const CSUP::DetectedObjectsMsgPtr&);
  void iterate();
  ~RWSDynObjs();

private:
  double time_step;
  webots::Supervisor* S;
  webots::Node* W; //pointer to webots world
  webots::Field* WC; //pointer to webots world children
  ros::Subscriber dynobjsub;
  ros::NodeHandle* n;
  std::vector<WDynObjMarker> WDOM;

  CSUP::DetectedObjectsMsg DynObjList;
  CSUP::DynamicObjectMsg DynObj;

};

RWSDynObjs::RWSDynObjs()
{

}
RWSDynObjs::~RWSDynObjs()
{

}

void RWSDynObjs::DynObjList_CallBack(const CSUP::DetectedObjectsMsgPtr& msg)
{
  DynObjList.header = msg->header;
  DynObjList.RobotID = msg->RobotID;
  DynObjList.RobotName = msg->RobotName;
  DynObjList.List = msg->List;
}

void RWSDynObjs::init(webots::Supervisor* S_in, ros::NodeHandle* n_in,string name)
{
  S = S_in;
  n = n_in;
  time_step = S->getBasicTimeStep();
  dynobjsub = n->subscribe(name, 1000, &RWSDynObjs::DynObjList_CallBack,this);
  W = S->getRoot();
  WC = W->getField("children");


}

void RWSDynObjs::iterate()
{
  WDOM.clear();
  WDOM.reserve(DynObjList.List.size());
  //Convert List into Objects in simulation.
  for (int i = 0;i<(int)DynObjList.List.size();i++)
  {
    WDynObjMarker M;

    WC->importMFNode(-1,"wb_models/marker.wbo");

    const webots::Node *n_aux = WC->getMFNode(WC->getCount());

    stringstream ss;
    DynObj = DynObjList.List[i];
    ss << "M_" << DynObj.ObjID;
    M.init(n_aux,ss.str());
    const double pos[3] = {(double)DynObj.Pos_x,(double)DynObj.Pos_y,(double)DynObj.Pos_z};
    M.set_position(pos);

    WDOM.push_back(M);

  }


}
